// Tencent is pleased to support the open source community by making ncnn available.
//
// Copyright (C) 2021 THL A29 Limited, a Tencent company. All rights reserved.
//
// Licensed under the BSD 3-Clause License (the "License"); you may not use this file except
// in compliance with the License. You may obtain a copy of the License at
//
// https://opensource.org/licenses/BSD-3-Clause
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.

#include "yolo.h"

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include "cpu.h"
#include<iostream>
#define MAX_STRIDE 32

static void slice(const ncnn::Mat& in, ncnn::Mat& out, int start, int end, int axis)
{
    ncnn::Option opt;
    opt.num_threads = 4;
    opt.use_fp16_storage = false;
    opt.use_packing_layout = false;

    ncnn::Layer* op = ncnn::create_layer("Crop");

    // set param
    ncnn::ParamDict pd;

    ncnn::Mat axes = ncnn::Mat(1);
    axes.fill(axis);
    ncnn::Mat ends = ncnn::Mat(1);
    ends.fill(end);
    ncnn::Mat starts = ncnn::Mat(1);
    starts.fill(start);
    pd.set(9, starts);// start
    pd.set(10, ends);// end
    pd.set(11, axes);//axes

    op->load_param(pd);

    op->create_pipeline(opt);

    // forward
    op->forward(in, out, opt);

    op->destroy_pipeline(opt);

    delete op;
}
static void interp(const ncnn::Mat& in, const float& scale, const int& out_w, const int& out_h, ncnn::Mat& out)
{
    ncnn::Option opt;
    opt.num_threads = 4;
    opt.use_fp16_storage = false;
    opt.use_packing_layout = false;

    ncnn::Layer* op = ncnn::create_layer("Interp");

    // set param
    ncnn::ParamDict pd;
    pd.set(0, 2);// resize_type
    pd.set(1, scale);// height_scale
    pd.set(2, scale);// width_scale
    pd.set(3, out_h);// height
    pd.set(4, out_w);// width

    op->load_param(pd);

    op->create_pipeline(opt);

    // forward
    op->forward(in, out, opt);

    op->destroy_pipeline(opt);

    delete op;
}
static void reshape(const ncnn::Mat& in, ncnn::Mat& out, int c, int h, int w, int d)
{
    ncnn::Option opt;
    opt.num_threads = 4;
    opt.use_fp16_storage = false;
    opt.use_packing_layout = false;

    ncnn::Layer* op = ncnn::create_layer("Reshape");

    // set param
    ncnn::ParamDict pd;

    pd.set(0, w);// start
    pd.set(1, h);// end
    if (d > 0)
        pd.set(11, d);//axes
    pd.set(2, c);//axes
    op->load_param(pd);

    op->create_pipeline(opt);

    // forward
    op->forward(in, out, opt);

    op->destroy_pipeline(opt);

    delete op;
}
static void sigmoid(ncnn::Mat& bottom)
{
    ncnn::Option opt;
    opt.num_threads = 4;
    opt.use_fp16_storage = false;
    opt.use_packing_layout = false;

    ncnn::Layer* op = ncnn::create_layer("Sigmoid");

    op->create_pipeline(opt);

    // forward

    op->forward_inplace(bottom, opt);
    op->destroy_pipeline(opt);

    delete op;
}
static float fast_exp(float x)
{
    union {
        uint32_t i;
        float f;
    } v{};
    v.i = (1 << 23) * (1.4426950409 * x + 126.93490512f);
    return v.f;
}
static float sigmoid(float x)
{
    return 1.0f / (1.0f + fast_exp(-x));
}

static void matmul(const std::vector<ncnn::Mat>& bottom_blobs, ncnn::Mat& top_blob)
{
    ncnn::Option opt;
    opt.num_threads = 2;
    opt.use_fp16_storage = false;
    opt.use_packing_layout = false;

    ncnn::Layer* op = ncnn::create_layer("MatMul");

    // set param
    ncnn::ParamDict pd;
    pd.set(0, 0);// axis

    op->load_param(pd);

    op->create_pipeline(opt);
    std::vector<ncnn::Mat> top_blobs(1);
    op->forward(bottom_blobs, top_blobs, opt);
    top_blob = top_blobs[0];

    op->destroy_pipeline(opt);

    delete op;
}
static float intersection_area(const Object& a, const Object& b)
{
    cv::Rect_<float> inter = a.rect & b.rect;
    return inter.area();
}

static void qsort_descent_inplace(std::vector<Object>& faceobjects, int left, int right)
{
    int i = left;
    int j = right;
    float p = faceobjects[(left + right) / 2].prob;

    while (i <= j)
    {
        while (faceobjects[i].prob > p)
            i++;

        while (faceobjects[j].prob < p)
            j--;

        if (i <= j)
        {
            // swap
            std::swap(faceobjects[i], faceobjects[j]);

            i++;
            j--;
        }
    }

    //     #pragma omp parallel sections
    {
        //         #pragma omp section
        {
            if (left < j) qsort_descent_inplace(faceobjects, left, j);
        }
        //         #pragma omp section
        {
            if (i < right) qsort_descent_inplace(faceobjects, i, right);
        }
    }
}

static void qsort_descent_inplace(std::vector<Object>& faceobjects)
{
    if (faceobjects.empty())
        return;

    qsort_descent_inplace(faceobjects, 0, faceobjects.size() - 1);
}

static void nms_sorted_bboxes(const std::vector<Object>& faceobjects, std::vector<int>& picked, float nms_threshold)
{
    picked.clear();

    const int n = faceobjects.size();

    std::vector<float> areas(n);
    for (int i = 0; i < n; i++)
    {
        areas[i] = faceobjects[i].rect.width * faceobjects[i].rect.height;
    }

    for (int i = 0; i < n; i++)
    {
        const Object& a = faceobjects[i];

        int keep = 1;
        for (int j = 0; j < (int)picked.size(); j++)
        {
            const Object& b = faceobjects[picked[j]];

            // intersection over union
            float inter_area = intersection_area(a, b);
            float union_area = areas[i] + areas[picked[j]] - inter_area;
            // float IoU = inter_area / union_area
            if (inter_area / union_area > nms_threshold)
                keep = 0;
        }

        if (keep)
            picked.push_back(i);
    }
}

static void generate_proposals(std::vector<GridAndStride> grid_strides, const ncnn::Mat& pred, float prob_threshold, std::vector<Object>& objects)
{
    const int num_points = grid_strides.size();
    const int num_class = 2;
    const int reg_max_1 = 16;

    for (int i = 0; i < num_points; i++)
    {
        const float* scores = pred.row(i) + 4 * reg_max_1;

        // find label with max score
        int label = -1;
        float score = -FLT_MAX;
        for (int k = 0; k < num_class; k++)
        {
            float confidence = scores[k];
            if (confidence > score)
            {
                label = k;
                score = confidence;
            }
        }
        float box_prob = sigmoid(score);
        if (box_prob >= prob_threshold)
        {
            ncnn::Mat bbox_pred(reg_max_1, 4, (void*)pred.row(i));
            {
                ncnn::Layer* softmax = ncnn::create_layer("Softmax");

                ncnn::ParamDict pd;
                pd.set(0, 1); // axis
                pd.set(1, 1);
                softmax->load_param(pd);

                ncnn::Option opt;
                opt.num_threads = 1;
                opt.use_packing_layout = false;

                softmax->create_pipeline(opt);

                softmax->forward_inplace(bbox_pred, opt);

                softmax->destroy_pipeline(opt);

                delete softmax;
            }

            float pred_ltrb[4];
            for (int k = 0; k < 4; k++)
            {
                float dis = 0.f;
                const float* dis_after_sm = bbox_pred.row(k);
                for (int l = 0; l < reg_max_1; l++)
                {
                    dis += l * dis_after_sm[l];
                }

                pred_ltrb[k] = dis * grid_strides[i].stride;
            }

            float pb_cx = (grid_strides[i].grid0 + 0.5f) * grid_strides[i].stride;
            float pb_cy = (grid_strides[i].grid1 + 0.5f) * grid_strides[i].stride;

            float x0 = pb_cx - pred_ltrb[0];
            float y0 = pb_cy - pred_ltrb[1];
            float x1 = pb_cx + pred_ltrb[2];
            float y1 = pb_cy + pred_ltrb[3];

            Object obj;
            obj.rect.x = x0;
            obj.rect.y = y0;
            obj.rect.width = x1 - x0;
            obj.rect.height = y1 - y0;
            obj.label = label;
            obj.prob = box_prob;
            obj.mask_feat.resize(32);
            std::copy(pred.row(i) + 64 + num_class, pred.row(i) + 64 + num_class + 32, obj.mask_feat.begin());
            objects.push_back(obj);
        }
    }
}
static void generate_grids_and_stride(const int target_w, const int target_h, std::vector<int>& strides, std::vector<GridAndStride>& grid_strides)
{
    for (int i = 0; i < (int)strides.size(); i++)
    {
        int stride = strides[i];
        int num_grid_w = target_w / stride;
        int num_grid_h = target_h / stride;
        for (int g1 = 0; g1 < num_grid_h; g1++)
        {
            for (int g0 = 0; g0 < num_grid_w; g0++)
            {
                GridAndStride gs;
                gs.grid0 = g0;
                gs.grid1 = g1;
                gs.stride = stride;
                grid_strides.push_back(gs);
            }
        }
    }
}
static void decode_mask(const ncnn::Mat& mask_feat, const int& img_w, const int& img_h,
                        const ncnn::Mat& mask_proto, const ncnn::Mat& in_pad, const int& wpad, const int& hpad,
                        ncnn::Mat& mask_pred_result)
{
    ncnn::Mat masks;
    matmul(std::vector<ncnn::Mat>{mask_feat, mask_proto}, masks);
    sigmoid(masks);
    reshape(masks, masks, masks.h, in_pad.h / 4, in_pad.w / 4, 0);
    slice(masks, mask_pred_result, (wpad / 2) / 4, (in_pad.w - wpad / 2) / 4, 2);
    slice(mask_pred_result, mask_pred_result, (hpad / 2) / 4, (in_pad.h - hpad / 2) / 4, 1);
    interp(mask_pred_result, 4.0, img_w, img_h, mask_pred_result);

}


Yolo::Yolo()
{
    blob_pool_allocator.set_size_compare_ratio(0.f);
    workspace_pool_allocator.set_size_compare_ratio(0.f);
}

int Yolo::load(const char* modeltype, int _target_size, const float* _mean_vals,  const float* _norm_vals, bool use_gpu)
{
    yolo.clear();
    blob_pool_allocator.clear();
    workspace_pool_allocator.clear();

    ncnn::set_cpu_powersave(2);
    ncnn::set_omp_num_threads(ncnn::get_big_cpu_count());

    yolo.opt = ncnn::Option();

#if NCNN_VULKAN
    yolo.opt.use_vulkan_compute = use_gpu;
#endif

    yolo.opt.num_threads = ncnn::get_big_cpu_count();
    yolo.opt.blob_allocator = &blob_pool_allocator;
    yolo.opt.workspace_allocator = &workspace_pool_allocator;

    char parampath[256];
    char modelpath[256];
    sprintf(parampath, "%s-seg.param", modeltype);
    sprintf(modelpath, "%s-seg.bin", modeltype);

    yolo.load_param(parampath);
    yolo.load_model(modelpath);

    target_size = _target_size;
    mean_vals[0] = _mean_vals[0];
    mean_vals[1] = _mean_vals[1];
    mean_vals[2] = _mean_vals[2];
    norm_vals[0] = _norm_vals[0];
    norm_vals[1] = _norm_vals[1];
    norm_vals[2] = _norm_vals[2];

    return 0;
}

int Yolo::load(AAssetManager* mgr, const char* modeltype, int _target_size, const float* _mean_vals, const float* _norm_vals, bool use_gpu)
{
    yolo.clear();
    blob_pool_allocator.clear();
    workspace_pool_allocator.clear();

    ncnn::set_cpu_powersave(2);
    ncnn::set_omp_num_threads(ncnn::get_big_cpu_count());

    yolo.opt = ncnn::Option();

#if NCNN_VULKAN
    yolo.opt.use_vulkan_compute = use_gpu;
#endif

    yolo.opt.num_threads = ncnn::get_big_cpu_count();
    yolo.opt.blob_allocator = &blob_pool_allocator;
    yolo.opt.workspace_allocator = &workspace_pool_allocator;

    char parampath[256];
    char modelpath[256];

    sprintf(parampath, "yolov8s-1024-%s-ncnn.param", modeltype);
    sprintf(modelpath, "yolov8s-1024-%s-ncnn.bin", modeltype);
    __android_log_print(ANDROID_LOG_DEBUG, "parampath", "yolov8s-1024-%s-ncnn.param",modeltype);
    yolo.load_param(mgr, parampath);
    yolo.load_model(mgr, modelpath);

    target_size = _target_size;
    mean_vals[0] = _mean_vals[0];
    mean_vals[1] = _mean_vals[1];
    mean_vals[2] = _mean_vals[2];
    norm_vals[0] = _norm_vals[0];
    norm_vals[1] = _norm_vals[1];
    norm_vals[2] = _norm_vals[2];

    return 0;
}

int Yolo:: detect(const cv::Mat& rgb, std::vector<Object>& objects)
{
    int width = rgb.cols;
    int height = rgb.rows;
    const float prob_threshold = 0.4f;
    const float nms_threshold = 0.5f;
    // pad to multiple of 32
    int w = width;
    int h = height;
    float scale = 1.f;
    if (w > h)
    {
        scale = (float)target_size / w;
        w = target_size;
        h = h * scale;
    }
    else
    {
        scale = (float)target_size / h;
        h = target_size;
        w = w * scale;
    }
    __android_log_print(ANDROID_LOG_DEBUG, "ncnn", "detect1");
    ncnn::Mat in = ncnn::Mat::from_pixels_resize(rgb.data, ncnn::Mat::PIXEL_BGR2RGB, width, height, w, h);

    // pad to target_size rectangle
    int wpad = (w + MAX_STRIDE-1) / MAX_STRIDE * MAX_STRIDE - w;
    int hpad = (h + MAX_STRIDE-1) / MAX_STRIDE * MAX_STRIDE - h;
    ncnn::Mat in_pad;
    ncnn::copy_make_border(in, in_pad, hpad / 2, hpad - hpad / 2, wpad / 2, wpad - wpad / 2, ncnn::BORDER_CONSTANT, 0.f);

    in_pad.substract_mean_normalize(0, norm_vals);

    ncnn::Extractor ex = yolo.create_extractor();

    ex.input("images", in_pad);

    ncnn::Mat out;
    ex.extract("output0", out);

    ncnn::Mat mask_proto;
    ex.extract("output1", mask_proto);
    std::vector<int> strides = {8, 16, 32}; // might have stride=64
    std::vector<GridAndStride> grid_strides;
    generate_grids_and_stride(in_pad.w, in_pad.h, strides, grid_strides);
    std::vector<Object> proposals;
    std::vector<Object> objects8;
    generate_proposals(grid_strides, out, prob_threshold, objects8);
    proposals.insert(proposals.end(), objects8.begin(), objects8.end());
    // sort all proposals by score from highest to lowest
    qsort_descent_inplace(proposals);
    // apply nms with nms_threshold
    std::vector<int> picked;
    nms_sorted_bboxes(proposals, picked, nms_threshold);
    int count = picked.size();

    ncnn::Mat mask_feat = ncnn::Mat(32, count, sizeof(float));
    for (int i = 0; i < count; i++) {
        float* mask_feat_ptr = mask_feat.row(i);
        std::memcpy(mask_feat_ptr, proposals[picked[i]].mask_feat.data(), sizeof(float) * proposals[picked[i]].mask_feat.size());
    }
    ncnn::Mat mask_pred_result;
    decode_mask(mask_feat, width, height, mask_proto, in_pad, wpad, hpad, mask_pred_result);
    objects.resize(count);
    for (int i = 0; i < count; i++)
    {
        objects[i] = proposals[picked[i]];

        // adjust offset to original unpadded
        float x0 = (objects[i].rect.x - (wpad / 2)) / scale;
        float y0 = (objects[i].rect.y - (hpad / 2)) / scale;
        float x1 = (objects[i].rect.x + objects[i].rect.width - (wpad / 2)) / scale;
        float y1 = (objects[i].rect.y + objects[i].rect.height - (hpad / 2)) / scale;

        // clip
        x0 = std::max(std::min(x0, (float)(width - 1)), 0.f);
        y0 = std::max(std::min(y0, (float)(height - 1)), 0.f);
        x1 = std::max(std::min(x1, (float)(width - 1)), 0.f);
        y1 = std::max(std::min(y1, (float)(height - 1)), 0.f);

        objects[i].rect.x = x0;
        objects[i].rect.y = y0;
        objects[i].rect.width = x1 - x0;
        objects[i].rect.height = y1 - y0;

        objects[i].mask = cv::Mat::zeros(height, width, CV_32FC1);
        cv::Mat mask = cv::Mat(height, width, CV_32FC1, (float*)mask_pred_result.channel(i));
        mask(objects[i].rect).copyTo(objects[i].mask(objects[i].rect));
    }
    return 0;
}

int Yolo::draw(cv::Mat& rgb, const std::vector<Object>& objects,double &s)
{
    static const char* class_names[] = {
            "oil", "druff"
    };
    static const unsigned char colors[81][3] = {
            {0,  0,   255},
            {225, 255, 255},
            {0,   94,  255},
            {0,   37,  255},
            {0,   255, 94},
            {255, 226, 0},
            {0,   18,  255},
            {255, 151, 0},
            {170, 0,   255},
            {0,   255, 56},
            {255, 0,   75},
            {0,   75,  255},
            {0,   255, 169},
            {255, 0,   207},
            {75,  255, 0},
            {207, 0,   255},
            {37,  0,   255},
            {0,   207, 255},
            {94,  0,   255},
            {0,   255, 113},
            {255, 18,  0},
            {255, 0,   56},
            {18,  0,   255},
            {0,   255, 226},
            {170, 255, 0},
            {255, 0,   245},
            {151, 255, 0},
            {132, 255, 0},
            {75,  0,   255},
            {151, 0,   255},
            {0,   151, 255},
            {132, 0,   255},
            {0,   255, 245},
            {255, 132, 0},
            {226, 0,   255},
            {255, 37,  0},
            {207, 255, 0},
            {0,   255, 207},
            {94,  255, 0},
            {0,   226, 255},
            {56,  255, 0},
            {255, 94,  0},
            {255, 113, 0},
            {0,   132, 255},
            {255, 0,   132},
            {255, 170, 0},
            {255, 0,   188},
            {113, 255, 0},
            {245, 0,   255},
            {113, 0,   255},
            {255, 188, 0},
            {0,   113, 255},
            {255, 0,   0},
            {0,   56,  255},
            {255, 0,   113},
            {0,   255, 188},
            {255, 0,   94},
            {255, 0,   18},
            {18,  255, 0},
            {0,   255, 132},
            {0,   188, 255},
            {0,   245, 255},
            {0,   169, 255},
            {37,  255, 0},
            {255, 0,   151},
            {188, 0,   255},
            {0,   255, 37},
            {0,   255, 0},
            {255, 0,   170},
            {255, 0,   37},
            {255, 75,  0},
            {0,   0,   255},
            {255, 207, 0},
            {255, 0,   226},
            {255, 245, 0},
            {188, 255, 0},
            {0,   255, 18},
            {0,   255, 75},
            {0,   255, 151},
            {255, 56,  0},
            {245, 255, 0}
    };
    int color_index = 0;
    for (size_t i = 0; i < objects.size(); i++)
    {
        const Object& obj = objects[i];

        const unsigned char* color = colors[color_index % 80];
        if(obj.label==0){
            color=colors[0];
        }else{
            color=colors[1];
        }
        color_index++;

        cv::Scalar cc(color[0], color[1], color[2]);

        fprintf(stderr, "%d = %.5f at %.2f %.2f %.2f x %.2f\n", obj.label, obj.prob,
                obj.rect.x, obj.rect.y, obj.rect.width, obj.rect.height);

        for (int y = 0; y < rgb.rows; y++) {
            uchar* image_ptr = rgb.ptr(y);
            const float* mask_ptr = obj.mask.ptr<float>(y);
            for (int x = 0; x < rgb.cols; x++) {
                if (mask_ptr[x] >= 0.5)
                {
                    image_ptr[0] = cv::saturate_cast<uchar>(image_ptr[0] * 0.5 + color[2] * 0.5);
                    image_ptr[1] = cv::saturate_cast<uchar>(image_ptr[1] * 0.5 + color[1] * 0.5);
                    image_ptr[2] = cv::saturate_cast<uchar>(image_ptr[2] * 0.5 + color[0] * 0.5);
                    s++;
                }
                image_ptr += 3;
            }
        }
        s = 1.0 * s / (rgb.rows * rgb.cols);
        cv::rectangle(rgb, obj.rect, cc, 2);

        char text[256];
        sprintf(text, "%s %.1f%%", class_names[obj.label], obj.prob * 100);

        int baseLine = 0;
        cv::Size label_size = cv::getTextSize(text, cv::FONT_HERSHEY_SIMPLEX, 0.5, 1, &baseLine);

        int x = obj.rect.x;
        int y = obj.rect.y - label_size.height - baseLine;
        if (y < 0)
            y = 0;
        if (x + label_size.width > rgb.cols)
            x = rgb.cols - label_size.width;

        cv::rectangle(rgb, cv::Rect(cv::Point(x, y), cv::Size(label_size.width, label_size.height + baseLine)),
                      cv::Scalar(255, 255, 255), -1);

        cv::putText(rgb, text, cv::Point(x, y + label_size.height),
                    cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 0));
    }

    return 0;
}
float dist(cv::Point p1, cv::Point p2) {	//友元函数实现
    double x = p1.x - p2.x;	//通过对象访问私有数据成员
    double y = p1.y - p2.y;
    return static_cast<float>(sqrt(x * x + y * y));}
double CalculateAngle(cv::Point Mar1Point, cv::Point Mar2Point) {
    double k= (double)(Mar2Point.y - Mar1Point.y) / (Mar2Point.x - Mar1Point.x);
    double arcLength1 = atan(k);
    double pi_value = acos(-1.0);
    double current_angle = arcLength1 * 180 /pi_value;

    /*current_angle = 30.0;*/
    return current_angle;
}
void rotate_txt(cv::Mat img_src, int angle, cv::Scalar color, double fontScale, std::string text, cv::Point start) {
    int fontFace = cv::FONT_HERSHEY_SIMPLEX;
    int thickness = 1;
    // 计算文本大小
    cv::Size textSize = cv::getTextSize(text, fontFace, fontScale, thickness, 0);

    // 生成带有倾斜文本的图像
    cv::Mat image_with_text = cv::Mat::zeros(img_src.rows, img_src.cols, CV_8UC1);
    cv::Mat image_with_color = cv::Mat::zeros(img_src.rows, img_src.cols, CV_8UC1);
    rectangle(image_with_color,cv::Point(start.x, start.y - textSize.height), cv::Point(start.x + textSize.width, start.y), cv::Scalar(255,255,255), -1);
    putText(image_with_text, text, start, fontFace, fontScale, cv::Scalar(255, 255, 255), thickness, cv::LINE_AA, false);
    // 创建仿射变换矩阵
    cv::Mat M = getRotationMatrix2D(start, angle, 1);
    cv::Mat M1 = getRotationMatrix2D(start, angle, 1);
    // 应用仿射变换
    warpAffine(image_with_text, image_with_text, M, image_with_text.size());
    warpAffine(image_with_color, image_with_color, M1, image_with_color.size());
    img_src.setTo(color, image_with_color);
    img_src.setTo(cv::Scalar(0,0,0), image_with_text);

}
void Yolo::DrawPred(cv::Mat &img, std::vector<Object> result, std::vector<std::string> classNames,
                    std::vector<cv::Scalar> color, const std::vector<unitInfo> &unitinfo) {
    cv::Mat mask = img.clone();
    /*for (int i = 0; i < result.size(); i++) {
        if((result[i].boxMask.rows&& result[i].boxMask.cols>0)&& result[i].id==0){
            mask(result[i].box).setTo(color[i], result[i].boxMask);
        }
    }*/
    for(int i=0; i<unitinfo.size(); i++){
//        if(unitinfo[i].unitCnt.area()>0){
//            circle(mask,unitinfo[i].unitPoint,20,Scalar(255,255,255),3);
////        rectangle(mask,unitinfo[i].unitCnt.tl(),unitinfo[i].unitCnt.br(),(0,255,0),2,8);
////        circle(mask,unitinfo[i].unitPoint,2,Scalar(0,0,255),2);
//        }
        std::vector<hairInfo> hairlist=unitinfo[i].hairlist;
        for(int j=0;j<hairlist.size();j++){
            cv::Scalar c;
            std::string status;
            if (hairlist[j].Diameter < 60) {
                c = color[0];
                status = "fine";
            }
            else if (hairlist[j].Diameter >= 60 && hairlist[j].Diameter <= 110) {
                c = color[1];
                status = "moderate";
            }
            else
            {
                c = color[2];
                status = "wide";
            }

//            if(hairlist[j].Diameter<30)
//                c=Scalar(2,1,252);
//            else  if((hairlist[j].Diameter>=30)&& (hairlist[j].Diameter<60))
//                c=Scalar(1, 255, 255);
//            else
//                c=Scalar(1 ,255, 0);
            line(mask,hairlist[j].hairCnt[0],hairlist[j].hairCnt[1],c,2,8);
            line(mask,hairlist[j].hairCnt[1],hairlist[j].hairCnt[2],c,2,8);
            line(mask,hairlist[j].hairCnt[2],hairlist[j].hairCnt[3],c,2,8);
            line(mask,hairlist[j].hairCnt[3],hairlist[j].hairCnt[0],c,2,8);
            std::vector<int> all_x;
            std::vector<int> all_y;
            for (int i = 0; i < 4; i++) {
                all_x.push_back(hairlist[j].hairCnt[i].x);
                all_y.push_back(hairlist[j].hairCnt[i].y);
            }
            float d1=dist(hairlist[j].hairCnt[0],hairlist[j].hairCnt[1]);
            float d2=dist(hairlist[j].hairCnt[3],hairlist[j].hairCnt[0]);
            cv::Point c1, c2, c1_1, c1_2, c2_1, c2_2;
            double angle;
            if(d1>d2){
                c1=(hairlist[j].hairCnt[0]+hairlist[j].hairCnt[1])/2;
                c2=(hairlist[j].hairCnt[2]+hairlist[j].hairCnt[3])/2;
                angle = CalculateAngle(c1,c2);
                if(hairlist[j].hairCnt[0].x==c1.x){
                    c1_1=cv::Point(c1.x,c1.y+10);
                    c1_2=cv::Point(c1.x,c1.y-10);
                }
                else{
                    c1_1=cv::Point(c1.x+1,(hairlist[j].hairCnt[0].y-c1.y)/(hairlist[j].hairCnt[0].x-c1.x)*1+c1.y);
                    c1_2=cv::Point(c1.x-1,-(hairlist[j].hairCnt[0].y-c1.y)/(hairlist[j].hairCnt[0].x-c1.x)*1+c1.y);
                }
                if(hairlist[j].hairCnt[3].x==c2.x){
                    c2_1=cv::Point(c2.x,c2.y+10);
                    c2_2=cv::Point(c2.x,c2.y-10);
                }
                else{
                    c2_1=cv::Point(c2.x+1,(hairlist[j].hairCnt[3].y-c2.y)/(hairlist[j].hairCnt[3].x-c2.x)*1+c2.y);
                    c2_2=cv::Point(c2.x-1,-(hairlist[j].hairCnt[3].y-c2.y)/(hairlist[j].hairCnt[3].x-c2.x)*1+c2.y);
                }
            }
            else{
                c1=(hairlist[j].hairCnt[0]+hairlist[j].hairCnt[3])/2;
                c2=(hairlist[j].hairCnt[1]+hairlist[j].hairCnt[2])/2;
                angle = CalculateAngle(c1, c2);
                if(hairlist[j].hairCnt[0].x==c1.x){
                    c1_1=cv::Point(c1.x,c1.y+10);
                    c1_2=cv::Point(c1.x,c1.y-10);
                }
                else{
                    c1_1=cv::Point(c1.x+1,(hairlist[j].hairCnt[0].y-c1.y)/(hairlist[j].hairCnt[0].x-c1.x)*1+c1.y);
                    c1_2=cv::Point(c1.x-1,-(hairlist[j].hairCnt[0].y-c1.y)/(hairlist[j].hairCnt[0].x-c1.x)*1+c1.y);
                }
                if(hairlist[j].hairCnt[1].x==c2.x){
                    c2_1=cv::Point(c2.x,c2.y+10);
                    c2_2=cv::Point(c2.x,c2.y-10);
                }
                else{
                    c2_1=cv::Point(c2.x+1,(hairlist[j].hairCnt[1].y-c2.y)/(hairlist[j].hairCnt[1].x-c2.x)*1+c2.y);
                    c2_2=cv::Point(c2.x-1,-(hairlist[j].hairCnt[1].y-c2.y)/(hairlist[j].hairCnt[1].x-c2.x)*1+c2.y);
                }
            }
            cv::Point start;
            if (angle * -1 > 0) {
                auto min_element_iter = min_element(all_x.begin(), all_x.end());
                start.x = *min_element_iter;
                start.y= all_y[distance(all_x.begin(),min_element_iter)];
            }
            else {
                auto min_element_iter = min_element(all_y.begin(), all_y.end());
                start.y = *min_element_iter;
                start.x = all_x[distance(all_y.begin(), min_element_iter)];
            }
            /*line(mask,c1_1,c1_2,color[unitinfo[i].unitIndex],2,8);
            line(mask,c2_1,c2_2,color[unitinfo[i].unitIndex],2,8);*/
            /*circle(mask, c1, 2, Scalar(0, 0, 255), 2);
            circle(mask, c2, 2, Scalar(0, 255, 0), 2);*/
            /*line(mask,c1,c2,color[unitinfo[i].unitIndex],2,8);*/

            /*string label=to_string(int(angle*-1));
            if(c2.x>c1.x)
                putText(img, label, c2, FONT_HERSHEY_SIMPLEX, 1, color[unitinfo[i].unitIndex], 2);
            else
                putText(img, label, c1, FONT_HERSHEY_SIMPLEX, 1, color[unitinfo[i].unitIndex], 2);*/
            rotate_txt(mask, int(angle*-1), c, 1.2, status, start);
        }
    }
    addWeighted(img, 0.5, mask, 0.5, 0, img); //add mask to src

//    imshow("1", img);
//    waitKey();
}
bool filter(cv::Rect box,std::vector<unitInfo>& unitinfo){  //判断结果是否已经被覆盖
    float area_new=box.area();
    cv::Rect inter;
    for(int i=0;i<unitinfo.size();i++){
        inter=box & unitinfo[i].unitCnt;
        float area_i=unitinfo[i].unitCnt.area();
        float area_inter=inter.area();

        if((area_i>area_new)&&(area_inter/area_new>0.5)) //过滤
            return true;
        else if((area_new>area_i)&&(area_inter/area_i>0.5)){ //替换
            unitinfo.erase(unitinfo.begin()+i);
            i-=1;
        }
    }

    return false;
}
void minBinaryImgAreaRect(cv::Mat &mask, cv::Mat &src, struct Object& seg) {
    cv::Mat binary;
    cv::threshold(mask,binary,100,255,cv::THRESH_BINARY | cv::THRESH_OTSU);
    std::vector<std::vector<cv::Point>> contours;
    cv::findContours(binary,contours,cv::RETR_LIST,cv::CHAIN_APPROX_NONE);
    //2.确定最小外接矩形
    //2.1 定义RotatedRect类型的vector容器rotatedRects存放最小的外接矩形，初始化大小为轮廓的个数。
    std::vector<cv::RotatedRect> rotatedRects(contours.size());
    //筛选轮廓面积
    double max_area=-1;
    int index=-1;
    for(int i=0;i<contours.size();i++){
        double area=cv::contourArea(contours[i]);
        if(area>max_area){
            max_area=area;
            index=i;
        }
    }
    if(index!=-1){
        //2.3 由轮廓(点集)确定出最小外接矩形
        rotatedRects[index]=cv::minAreaRect(contours[index]);
        //2.31 旋转矩形类RotatedRect中有Point()方法，参数Point2f* pts，将旋转矩形的四个端点存储进pts.
        cv::Point2f pts[4]={cv::Point (0,0)};
        rotatedRects[index].points(pts);

        //2.4 画出最小外接矩形
        cv::RNG rng(time(0));
        cv::Scalar color=cv::Scalar (rng.uniform(0,255),rng.uniform(0,255),rng.uniform(0,255));

        seg.minbox.push_back(pts[0]);
        seg.minbox.push_back(pts[1]);
        seg.minbox.push_back(pts[2]);
        seg.minbox.push_back(pts[3]);
    }
}
void pcaAnalyze(const std::vector<cv::Point> &points,cv::Mat &result,const cv::Rect& boundRect, hairInfo& singlehair){
    int size = points.size();
    cv::Mat data_pts = cv::Mat(size,2,CV_64FC1);
    for(int i=0;i<size;i++){
        data_pts.at<double>(i,0) = points[i].x;
        data_pts.at<double>(i,1) = points[i].y;
    }
//    cout<<"开始pca分析"<<endl;
    //使用PCA进行分析
    cv::PCA pca_analyze(data_pts,cv::Mat(),cv::PCA::DATA_AS_ROW);//实例化PCA
    //找出轮廓的圆心：均值数据的第一个值即为数据中心点
    cv::Point center = cv::Point(pca_analyze.mean.at<double>(0,0),pca_analyze.mean.at<double>(0,1));
    singlehair.hairCenter=center;
//    cout<<center<<endl;
    //绘制轮廓中心点
//    circle(result,center,3,Scalar(0,0,255),3,LINE_8);
//    cout<<"准备利用pca寻找轮廓方向"<<endl;
    //找出并绘制出轮廓方向
    std::vector<cv::Point2d> vecs(2);//特征向量
    std::vector<double> vals(2);//特征值
    for(int i=0;i<2;i++){
        vals[i] = pca_analyze.eigenvalues.at<double>(i,0);
        vecs[i] = cv::Point2d(pca_analyze.eigenvectors.at<double>(i,0),pca_analyze.eigenvectors.at<double>(i,1));
    }

    //求出主方向直线和外接矩形的交点,(毛发长度
    float k0 = vecs[0].y/vecs[0].x; //斜率
    cv::Point2f pt1_0 = cv::Point2f(boundRect.x,k0*(boundRect.x - center.x)+center.y);
    cv::Point2f pt2_0 = cv::Point2f((boundRect.x+boundRect.width),k0*((boundRect.x+boundRect.width)-center.x)+center.y);
//    circle(result,pt1,5,Scalar(0,255,255),-1);
//    circle(result,pt2,5,Scalar(0,255,255),-1);
    //遍历两个交点之间的线段，得出和轮廓的交点
    cv::LineIterator it0(result, pt1_0, pt2_0, 8);
    std::vector<cv::Point> pts0;
    for(int i = 0; i < it0.count; i++, ++it0){
        cv::Point pt(it0.pos());//获得线段上的点
        if (abs(pointPolygonTest(points,pt,true)) < 1){
            pts0.push_back(pt);
//         circle(result,pt,5,Scalar(0,0,255),-1);
        }}

    singlehair.hairFoot=pts0.front();
    singlehair.hairIndia=pts0.back();
    float l=dist(singlehair.hairFoot,singlehair.hairIndia);
    singlehair.length=l;

    //求出副方向直线和外接矩形的交点,(毛发直径
    float k = vecs[1].y/vecs[1].x; //斜率
    cv::Point2f pt1 = cv::Point2f(boundRect.x,k*(boundRect.x - center.x)+center.y);
    cv::Point2f pt2 = cv::Point2f((boundRect.x+boundRect.width),k*((boundRect.x+boundRect.width)-center.x)+center.y);
//    circle(result,pt1,5,Scalar(0,255,255),-1);
//    circle(result,pt2,5,Scalar(0,255,255),-1);
    //遍历两个交点之间的线段，得出和轮廓的交点
    cv::LineIterator it(result, pt1, pt2, 8);
    std::vector<cv::Point> pts;
    for(int i = 0; i < it.count; i++, ++it){
        cv::Point pt(it.pos());//获得线段上的点
        if (abs(pointPolygonTest(points,pt,true)) < 1){
            pts.push_back(pt);
//         circle(result,pt,5,Scalar(0,0,255),-1);
        }}

    cv::Point p1=pts.front();
    cv::Point p2=pts.back();
    float d=dist(p1,p2);
    singlehair.Diameter=d;

}
void Yolo::info_init(cv::Mat &img, std::vector<Object> &result, std::vector<unitInfo> &unitinfo,
                     std::vector<hairInfo> &hairinfo, const float &ratio) {
    int row=img.rows;
    int col=img.cols;
    cv::Scalar color=cv::Scalar(255,255,255);
    __android_log_print(ANDROID_LOG_DEBUG, "ncnn", "diameter=%d",1);
    for(int i=0;i<result.size();i++){
        cv::Mat mask(row,col,CV_8UC1,cv::Scalar(0));
        if(result[i].mask(result[i].rect).rows && result[i].mask(result[i].rect).cols>0){
            //毛囊单元初始化，获取毛囊单元的发根重心，毛囊轮廓
            if(result[i].label==1){
                if(filter(result[i].rect,unitinfo))
                    continue;
                else{
                    __android_log_print(ANDROID_LOG_DEBUG, "ncnn", "diameter=%d",3);
                    unitInfo singleunit;
                    singleunit.unitIndex=unitinfo.size();
                    singleunit.unitCnt=result[i].rect;
                    singleunit.unitPoint.x=result[i].rect.x+(result[i].rect.width/2);
                    singleunit.unitPoint.y=result[i].rect.y+(result[i].rect.height/2);
                    unitinfo.push_back(singleunit);
                }
            }else if(result[i].label==0){
                __android_log_print(ANDROID_LOG_DEBUG, "ncnn", "diameter=%d",4);
                __android_log_print(ANDROID_LOG_DEBUG, "ncnn", "channel=%d",result[i].mask.channels());
                __android_log_print(ANDROID_LOG_DEBUG, "ncnn", "rect=%d",mask(result[i].rect).cols);
                __android_log_print(ANDROID_LOG_DEBUG, "ncnn", "rect=%d",mask(result[i].rect).rows);
                __android_log_print(ANDROID_LOG_DEBUG, "ncnn", "mask=%d",result[i].mask.cols);
                __android_log_print(ANDROID_LOG_DEBUG, "ncnn", "mask=%d",result[i].mask.rows);
                __android_log_print(ANDROID_LOG_DEBUG, "ncnn", "maskrect=%d",result[i].mask(result[i].rect).cols);
                __android_log_print(ANDROID_LOG_DEBUG, "ncnn", "maskrect=%d",result[i].mask(result[i].rect).rows);
                for (int y = 0; y < mask.rows; y++) {
                    uchar* image_ptr = mask.ptr<uchar>(y);
                    const float* mask_ptr = result[i].mask.ptr<float>(y);
                    for (int x = 0; x < mask.cols; x++) {
                        if (mask_ptr[x] >= 0.5)
                        {
                            image_ptr[x]=255;
                        }

                    }
                }
//                mask(result[i].rect).setTo(color,result[i].mask(result[i].rect));
                __android_log_print(ANDROID_LOG_DEBUG, "ncnn", "diameter=%d",5);
                minBinaryImgAreaRect(mask,img,result[i]);
                cv::Mat binary;
                threshold(mask, binary, 100, 255, cv::THRESH_BINARY | cv::THRESH_OTSU);
                //发现轮廓
                std::vector<std::vector<cv::Point>> contours;
                findContours(binary,contours,cv::RETR_LIST,cv::CHAIN_APPROX_NONE);
                int index=0;
                double max_area=-1;
                //寻找最大凸包
                for(size_t j = 0;j<contours.size();j++){
                    double area = contourArea(contours[j]);
                    if ((area <10000)&&(area>max_area)) {
                        max_area=area;
                        index=j;
                    }
                }
                if (max_area!=-1){
                    //                Rect r = boundingRect(Mat(contours[index]));
                    //                rectangle(img, r, Scalar(255), 2);
                    hairInfo singlehair;
                    singlehair.hairIndex=hairinfo.size();
                    __android_log_print(ANDROID_LOG_DEBUG, "ncnn", "diameter=%d",2);
                    //                singlehair.hairCnt=contours[index];
                    singlehair.hairCnt=result[i].minbox;
                    pcaAnalyze(contours[index],img,result[i].rect,singlehair);
                    singlehair.Diameter=ratio*singlehair.Diameter;
                    hairinfo.push_back(singlehair);
                }
            }
        }
    }
}
std::vector<point> getpoint(const std::vector<hairInfo>& hairinfo){
    std::vector<point> data;
    int i=0;
    int s=hairinfo.size();
    while (i<s) {
        point p(hairinfo[i].hairFoot.x,hairinfo[i].hairFoot.y,i);
        data.push_back(p);
        i+=1;
    }
    return data;
}

float squareDistance(point a,point b){
    return sqrt((a.x-b.x)*(a.x-b.x)+(a.y-b.y)*(a.y-b.y));
}
#include <fstream>
#include <stack>
void DBSCAN(const std::vector<hairInfo>& hairinfo, std::vector<unitInfo>& unitinfo, float Eps,int MinPts){
    std::vector<point> dataset = getpoint(hairinfo);
    int len = dataset.size();
    //calculate pts
    for(int i=0;i<len;i++){
        for(int j=i+1;j<len;j++){
            if(squareDistance(dataset[i],dataset[j])<Eps)
                dataset[i].pts++;
            dataset[j].pts++;
        }
    }

    //core point
    std::vector<point> corePoint;
    for(int i=0;i<len;i++){
        if(dataset[i].pts>=MinPts) {
            dataset[i].pointType = 3;
            corePoint.push_back(dataset[i]);
        }
    }

    //joint core point
    for(int i=0;i<corePoint.size();i++){
        for(int j=i+1;j<corePoint.size();j++){
            if(squareDistance(corePoint[i],corePoint[j])<Eps){
                corePoint[i].corepts.push_back(j);
                corePoint[j].corepts.push_back(i);
            }
        }
    }
    for(int i=0;i<corePoint.size();i++){
        std::stack<point*> ps;
        if(corePoint[i].visited == 1) continue;
        ps.push(&corePoint[i]);
        point *v;
        while(!ps.empty()){
            v = ps.top();
            v->visited = 1;
            ps.pop();
            for(int j=0;j<v->corepts.size();j++){
                if(corePoint[v->corepts[j]].visited==1) continue;
                corePoint[v->corepts[j]].cluster = corePoint[i].cluster;
                corePoint[v->corepts[j]].visited = 1;
                ps.push(&corePoint[v->corepts[j]]);
            }
        }
    }
    //border point,joint border point to core point
    for(int i=0;i<len;i++){
        if(dataset[i].pointType==3) continue;
        for(int j=0;j<corePoint.size();j++){
            if(squareDistance(dataset[i],corePoint[j])<Eps) {
                dataset[i].pointType = 2;
                dataset[i].cluster = corePoint[j].cluster;
                break;
            }
        }
    }


    //output
    for(int i=0;i<len;i++){
        if(dataset[i].pointType == 2){
            std::cout<<dataset[i].x<<","<<dataset[i].y<<","<<dataset[i].cluster<<std::endl;
        }
    }

    int l=unitinfo.size();
    for(int i=0;i<corePoint.size();i++){
        bool _new=true;
        for(int j=0;j<unitinfo.size();j++){
            if (corePoint[i].cluster+l==unitinfo[j].unitIndex){
//                cout<<"unitinfo["<<unitinfo[j].unitIndex<<"]:"<<"hairinfo["<<i<<"]:"<<hairinfo[i].hairCnt[0].x<<endl;
                unitinfo[j].hairlist.push_back(hairinfo[i]);
                unitinfo[j].unitPoint=(unitinfo[j].unitPoint+hairinfo[i].hairCenter)/2;
                _new=false;}
        }
        if (_new){
            unitInfo unit;
            unit.unitIndex=corePoint[i].cluster+l;
            unit.hairlist.push_back(hairinfo[i]);
            unit.unitPoint=hairinfo[i].hairCenter;
            unitinfo.push_back(unit);
        }
    }

    for(int i=0;i<unitinfo.size();i++)
        unitinfo[i].unitIndex=i;
}
//毛发毛囊匹配
void Yolo::info_make(std::vector<hairInfo> &hairinfo, std::vector<unitInfo> &unitinfo,
                     totalInfo &totalinfo, float Eps, int MinPts) {
    std::vector<hairInfo> rest_hairinfo;
    //毛发和毛囊匹配
    for(int i=0;i<hairinfo.size();i++){
        double min_distance=1000000000000000;
        int unitIndex=-1;
        totalinfo.meanDiameter+=hairinfo[i].Diameter;
        for(int j=0;j<unitinfo.size();j++){
            if((unitinfo[j].unitCnt.contains(hairinfo[i].hairFoot)) || (unitinfo[j].unitCnt.contains(hairinfo[i].hairIndia))){
                if(unitinfo[j].unitCnt.contains(hairinfo[i].hairFoot)){
                    float distance=dist(hairinfo[i].hairFoot,unitinfo[j].unitPoint);
                    if(distance<min_distance){
                        min_distance=distance;
                        unitIndex=j;
                    }
                }
                if (unitinfo[j].unitCnt.contains(hairinfo[i].hairIndia)){
                    float distance=dist(hairinfo[i].hairIndia,unitinfo[j].unitPoint);
                    if(distance<min_distance){
                        min_distance=distance;
                        unitIndex=j;
                        cv::Point p=hairinfo[i].hairFoot;
                        hairinfo[i].hairFoot=hairinfo[i].hairIndia;
                        hairinfo[i].hairIndia=p;
                    }
                }
            }
            else{
                for(int k=0;k<hairinfo[i].hairCnt.size();k++){
                    if(unitinfo[j].unitCnt.contains(hairinfo[i].hairCnt[k])){
                        float d1=dist(hairinfo[i].hairFoot,hairinfo[i].hairCnt[k]);
                        float d2=dist(hairinfo[i].hairIndia,hairinfo[i].hairCnt[k]);
                        if(d1<=d2){
                            float distance=dist(hairinfo[i].hairFoot,unitinfo[j].unitPoint);
                            if(distance<min_distance){
                                min_distance=distance;
                                unitIndex=j;
                            }
                        }
                        else{
                            float distance=dist(hairinfo[i].hairIndia,unitinfo[j].unitPoint);
                            if(distance<min_distance){
                                min_distance=distance;
                                unitIndex=j;
                                cv::Point p=hairinfo[i].hairFoot;
                                hairinfo[i].hairFoot=hairinfo[i].hairIndia;
                                hairinfo[i].hairIndia=p;
                            }
                        }
                    }
                }
            }
        }
        if (unitIndex!=-1)
            unitinfo[unitIndex].hairlist.push_back(hairinfo[i]);
        else
            rest_hairinfo.push_back(hairinfo[i]);
    }
    //对未匹配的毛发进行DBSCAN聚类
    if (rest_hairinfo.size()>1)
        DBSCAN(rest_hairinfo,unitinfo,Eps,MinPts);
    else if(rest_hairinfo.size()==1){
        unitInfo unit;
        unit.unitIndex=unitinfo.size();
        unit.hairlist.push_back(rest_hairinfo[0]);
        unit.unitPoint=rest_hairinfo[0].hairCenter;
        unitinfo.push_back(unit);
    }

    //获取总体指标：毛发数量，平均直径，毛囊数量
    totalinfo.hairNum=hairinfo.size();
    totalinfo.meanDiameter=totalinfo.meanDiameter/totalinfo.hairNum;
    totalinfo.unitNum=unitinfo.size();

}
